#pragma config(Sensor, in1,    Potentiometer,  sensorPotentiometer)
#pragma config(Sensor, dgtl1,  front_switch,   sensorTouch)
#pragma config(Sensor, dgtl2,  back_switch,    sensorTouch)
#pragma config(Motor,  port1,           left_motor,    tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port6,           claw,          tmotorVex393, openLoop)
#pragma config(Motor,  port7,           arm,           tmotorVex393, openLoop)
#pragma config(Motor,  port10,          right_motor,   tmotorVex393, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
	bool enable_robot=false;

	float scale=1;

	while( true )
	{
		if( vexRT[Btn7U] && vexRT[Btn8U] )
		{
			enable_robot=true;
		}

		if( vexRT[Btn7D] && vexRT[Btn8D] )
		{
			enable_robot=false;

			motor[arm]=0;
			motor[claw]=0;
			motor[left_motor]=0;
			motor[right_motor]=0;
		}

		if( enable_robot )
		{
			while(true)
			{
				if( vexRT[Btn8L] )
				{
					scale=scale+0.5;
					while( vexRT[Btn8L])
					{
					}
				}

				if( vexRT[Btn8R] )
				{
					scale=scale-0.5;
					while( vexRT[Btn8R] )
					{
					}

					if(scale < 1 )
					{
						scale = 1;
					}
				}

				motor[left_motor]=(vexRT[Ch3]+vexRT[Ch1])/scale;
				motor[right_motor]=((vexRT[Ch3]-vexRT[Ch1])*.977)/scale;

				while( vexRT[ Btn7L ] )
				{
					if(SensorValue[front_switch] && !SensorValue[back_switch])
					{
						motor[left_motor]=0;
						motor[right_motor]=62;
					}
					else if(!SensorValue[front_switch] && SensorValue[back_switch])
					{
						motor[left_motor]=64;
						motor[right_motor]=0;
					}
					else if(SensorValue[front_switch] && SensorValue[back_switch])
					{
						motor[left_motor]=64;
						motor[right_motor]=62;
					}
					else
					{
						motor[left_motor]=63;
						motor[right_motor]=0;
						wait1Msec(4);
						motor[right_motor]=62;
						motor[left_motor]=0;
						wait1Msec(2);
					}
				}
				if( vexRT[ Btn6U ] )
				{
					motor[ arm ] = -127;
				}
				else if( vexRT[ Btn6D ] )
				{
					motor[ arm ] = 127;
				}


				else
				{
					motor[ arm ] = -10;
				}

				if( vexRT[ Btn5D ] )
				{
					motor[ claw ] = 127;
				}
				else if( vexRT[ Btn5U ] )
				{
					motor[ claw ] = -127;
				}
				else
				{
					motor[ claw ] = 0;
				}
			}
		}
	}
}
